#include <config.h>

.global _start
.global main
.global _main

_start:
    b reset

#define CONFIG_ARM64
#ifdef CONFIG_ARM64
/*
 * Register aliases.
 */
lr  .req    x30

/*
 * Branch according to exception level
 */
.macro  branch_if_el2, xreg, el2_label
    mrs \xreg, CurrentEL
    cmp \xreg, 0x8
    b.eq    \el2_label
.endm

/*
 * Branch if current processor is a slave,
 * choose processor with all zero affinity value as the master.
 */
.macro  branch_if_slave, xreg, slave_label
    mrs \xreg, mpidr_el1
    tst \xreg, #0xff        /* Test Affinity 0 */
    b.ne    \slave_label
    lsr \xreg, \xreg, #8
    tst \xreg, #0xff        /* Test Affinity 1 */
    b.ne    \slave_label
    lsr \xreg, \xreg, #8
    tst \xreg, #0xff        /* Test Affinity 2 */
    b.ne    \slave_label
    lsr \xreg, \xreg, #16
    tst \xreg, #0xff        /* Test Affinity 3 */
    b.ne    \slave_label
.endm

#endif /* CONFIG_ARM64 */



reset:

    //
    // program the VBARs
    //
    ldr x1, =deal_excpt
    msr VBAR_EL1, x1

    ldr x1, =deal_excpt
    msr VBAR_EL2, x1

    ldr x1, =deal_excpt
    msr VBAR_EL3, x1
    

    msr SCR_EL3, xzr  // Ensure NS bit is initially clear, so secure copy of ICC_SRE_EL1 can be configured
    isb
    //
    // neither EL3 nor EL2 trap floating point or accesses to CPACR
    //
    msr CPTR_EL3, xzr
    msr CPTR_EL2, xzr

    mov x0, #1 << 8
    msr CPTR_EL3, x0            /* sve trap */

	mov	x0, #3 << 20
    mov x1, #3 << 16            /* sve Enable */
    orr x0, x0, x1                  
	msr	cpacr_el1, x0			/* Enable FP/SIMD */
	msr	mdscr_el1, xzr

    branch_if_slave x0, slave_cpu

master:
    // mov x0, #0x8fff0000         /*init sp*/
    mov x0, #STACK_ADDR
    mov sp, x0

	bl mov_mem					/*move to mem run*/
	ldr x0, =_main
	br x0
	
loop:
	b loop
	
_main:
	b main

slave_cpu:
    wfi
    // b slave_cpu